#include "AODAlgo.h"
#include "MedianBackground.h"

#include <mutex>

const float learn_rate_one = 1.01;
const float learn_rate_two = 1.001;
const int MEDIAN_DIFFERENCE_THRESHOLD = 50;
RNG rng(12345);

// default roi
cv::Rect roi(300, 200, 120, 60);

class AODAlgoPrivate {

public:
  AODAlgoPrivate() {
    static int i = 0;
    i++;
    name = to_string(i);
  };
  ~AODAlgoPrivate(){};

  std::once_flag of;

  cv::Rect roi;

  string name;
  // Mat frame_init = imread("/home/sukibean/data/init.jpg")(roi);
  MedianBackground *medianBgOne;
  MedianBackground *medianBgTwo;

  Mat medianBgImageOne, medianBgImageTwo, medianDifference;
};

void binary_closing_operation(Mat *image) {
  Mat five_by_five_element(5, 5, CV_8U, Scalar(1));
  morphologyEx(*image, *image, MORPH_CLOSE, five_by_five_element);
}

void binary_opening_operation(cv::Mat *image) {
  Mat five_by_five_element(5, 5, CV_8U, Scalar(1));
  morphologyEx(*image, *image, MORPH_OPEN, five_by_five_element);
}

AODAlgo::AODAlgo() : m_pHandle(new AODAlgoPrivate()) {}
AODAlgo::~AODAlgo() {}

void AODAlgo::setROI(cv::Rect rt) { m_pHandle->roi = rt; };

void AODAlgo::detect(const AODInput &input, AODResult& result){
  cv::Mat matDetect = input.mat;
  if (matDetect.empty()) {
    assert("AODAlgo detect mat empty");
  }

  cv::Rect rtROI = roi;
  if (m_pHandle->roi.area() > 0) {
    rtROI = m_pHandle->roi;
  }

  cv::Mat current_frame = input.mat(rtROI);

  std::call_once(m_pHandle->of, [&]() {
    m_pHandle->medianBgOne = new MedianBackground(current_frame, learn_rate_one);
    m_pHandle->medianBgTwo = new MedianBackground(current_frame, learn_rate_two);
  });

  m_pHandle->medianBgOne->UpdateBackground(current_frame);
  m_pHandle->medianBgTwo->UpdateBackground(current_frame);
  m_pHandle->medianBgImageOne = m_pHandle->medianBgOne->GetBackgroundImage();
  m_pHandle->medianBgImageTwo = m_pHandle->medianBgTwo->GetBackgroundImage();

  // get the binary difference image
  absdiff(m_pHandle->medianBgImageOne, m_pHandle->medianBgImageTwo,
          m_pHandle->medianDifference);
  cvtColor(m_pHandle->medianDifference, m_pHandle->medianDifference,
           CV_BGR2GRAY);
  threshold(m_pHandle->medianDifference, m_pHandle->medianDifference,
            MEDIAN_DIFFERENCE_THRESHOLD, 255, THRESH_BINARY);

  // clean the binary difference image
  binary_closing_operation(&m_pHandle->medianDifference);
  binary_opening_operation(&m_pHandle->medianDifference);

  vector<vector<Point>> contours;
  vector<Vec4i> hierarchy;
  findContours(m_pHandle->medianDifference, contours, hierarchy, CV_RETR_TREE,
               CV_CHAIN_APPROX_NONE);

  vector<Rect> boundRect(contours.size());

  for (int idx = 0; idx < boundRect.size(); idx++) {
    boundRect[idx] = cv::boundingRect(Mat(contours[idx]));
  }

  // 面积大小排序
  sort(boundRect.begin(), boundRect.end(),
       [](const cv::Rect rt1, const cv::Rect rt2) {
         return rt1.area() > rt2.area();
       });

  // 面积大小阈值
  // if (boundRect.size() > 0 && boundRect[0].area() > 400) {
  if (boundRect.size() > 0) {
    result.rt = boundRect[0];

    // 显示
    // Scalar color = Scalar(255, 255, 0);
    // rectangle(m_pHandle->medianDifference, boundRect[0].tl(),
    // boundRect[0].br(),
    //           color, 2, 8, 0);
  }

  imshow(m_pHandle->name, m_pHandle->medianDifference);
  waitKey(1);
};

void AODAlgo::reset() {
  m_pHandle->medianBgOne->reset();
  m_pHandle->medianBgTwo->reset();
};